import rclpy
from rclpy.node import Node
import rclpy.time
from tf2_ros import TransformListener,Buffer
from tf_transformations import euler_from_quaternion



class TFListener(Node):

    def __init__(self):
        super().__init__("tf_listener")
        self.butter_=Buffer()
        self.listener_=TransformListener(self.butter_,self)
        self.timer_=self.create_timer(0.01,self.get_tf)
        

    def get_tf(self):
        """
        发布静态tf
        """
        try:
            result=self.butter_.lookup_transform("base_link","bottle_link",
                                                 rclpy.time.Time(seconds=0),
                                                 rclpy.time.Duration(seconds=0.1))
            tranfrom=result.transform
            self.get_logger().info(f"平移{tranfrom.translation}")
            self.get_logger().info(f"旋转{tranfrom.rotation}")
            rotation_euler=euler_from_quaternion(
                [tranfrom.rotation.x,
                tranfrom.rotation.y,
                tranfrom.rotation.z,
                tranfrom.rotation.w]
            )
            self.get_logger().info(f"旋转RPY{rotation_euler}")

        except Exception as e:
            self.get_logger().warn(f"错误:{e}")
        # 发布坐标
        


def main():
    rclpy.init()
    node=TFListener()
    rclpy.spin(node)
    rclpy.shutdown()
    

